#include "WPILib.h"
#include "Shooter.hpp"


// Constructor
Shooter::Shooter(UINT32 pwmCh, UINT32 motID):
	cameraServo(pwmCh), shooterMotor(motID, CANJaguar::kPercentVbus)
{
	//printf("Shooter constructor started\n");

	// Initialize camera servo
	SetCameraAngle(STARTING_ANGLE);
	cameraAngle = STARTING_ANGLE;
	cameraIterator = CAMERA_ITERATOR;
	motorSetpoint = 0;
	ManualControl(motorSetpoint);
	
	// Initialize the speedTable (!!!table is expected to increase in tilt angle as you go down!!!)
	// The concept here is that the angle will be low when the robot is far from the target (camera looking forward)
	// 	and the angle will be high (camera looking up) when the robot is close to the target

	// These values are to be determined empirically (by experiment)
	// 1) Set the robot at some desired position from the target (say 20ft)
	// 2) Adjust the camera until the target is in view
	// 3) Manually set the speed of the shooter motor until balls fly the proper distance
	// 4) Record the camera angle and speed to be entered here (build this table from low angle to high)
	// 5) Move the robot closer or farther by some desired amount (a foot or two) and repeat

	speedTable[0][0] = 0;	// Angle, 0 meaning that the camera is looking straight ahead
	speedTable[0][1] = 75;	// Speed in % max speed

	speedTable[1][0] = 10;
	speedTable[1][1] = 70;

	speedTable[2][0] = 20;
	speedTable[2][1] = 65;

	speedTable[3][0] = 30;
	speedTable[3][1] = 60;

	speedTable[4][0] = 40;
	speedTable[4][1] = 55;

	speedTable[5][0] = 50;
	speedTable[5][1] = 50;

	speedTable[6][0] = 60;
	speedTable[6][1] = 45;

	speedTable[7][0] = 70;
	speedTable[7][1] = 40;

	speedTable[8][0] = 80;
	speedTable[8][1] = 35;

	speedTable[9][0] = 90;
	speedTable[9][1] = 25;
}

// Control the shooter motor by camera tilt angle
// cameraY - value from -1 - +1 from the joystick camera tilt angle controller
void Shooter::ControlByCamera(float cameraY)
{
	if(--cameraIterator <= 0) // Don't react to every call, could average the values but I don't think that is neccessary
	{
		cameraAngle -= cameraY; // Note: using the opposite of the value for joystick

		if(cameraAngle > MAX_ANGLE)
		{
			cameraAngle = MAX_ANGLE;
		}
		else if(cameraAngle < MIN_ANGLE)
		{
			cameraAngle = MIN_ANGLE;
		}

		SetCameraAngle(cameraAngle);

		// Set shooter motor based on camera tilt angle
		motorSetpoint = SpeedLookup(cameraAngle)/100;
		shooterMotor.Set(motorSetpoint);
		
		// Debug printout
		if(iCount<=0)
		{
			iCount=200;
			printf("CameraAngle = %0.2f, motorSetpoint = %0.2f\n", cameraAngle, motorSetpoint*100);
		}
	
		cameraIterator = CAMERA_ITERATOR;
	}
}

// Set the shooter motor speed manually
// Can be used to turn off shooter motor (set speed to 0)
void Shooter::ManualControl(float speed)
{
	shooterMotor.Set(speed); // Speed as a % max speed
}

// Set the shooter motor speed manually but incrementally
void Shooter::ManualControlIncremental(float incSpeed)
{
	if(motorSetpoint < -1)
	{
		motorSetpoint = -1;
	}
	else if(motorSetpoint > 1)
	{
		motorSetpoint = 1;
	}	
	else
	{
		motorSetpoint -= incSpeed/100;		
	}
	
	shooterMotor.Set(motorSetpoint);
}
void Shooter::PrintFF()
{
		if(iCount<=0)
	{
			printf("CameraAngle = %0.2f, motorSetpoint = %0.2f\n", cameraAngle, motorSetpoint*100);
			iCount=100;
	}
	iCount--;

}

bool Shooter::IsSafe()
{
	return motorSetpoint>SAFESPEED;
}
// Sets the internal camera tilt angle
void Shooter::CalibrateCameraAngle(float angle)
{
	//float tempAngle = cameraServo.GetAngle();
	cameraAngleCal = cameraAngle - angle;
	//cameraAngle = angle;
}

// Returns the camera tilt angle
float Shooter::GetCameraAngle()
{
	return(cameraAngle);
}



// Set the camera's angle. This routine knows how to convert the angle into what the servo needs
// to go to the that angle
void Shooter::SetCameraAngle(float angle)
{
	// Adjust camera tilt angle (Convention is 0deg is horizontal (looking forward) and +90deg is vert (looking up)
	// Notes from testing:
	// 1) Servo motor goes oposite direction that we want so mult angle by -1
	// 2) Servo motor angle needs to be adjusted by 78deg to get it to aline our conventions
	// 3) Servo motor travels 0.5deg when we tell it to move 1deg so mult angle by 2
	angle += cameraAngleCal;
	float adjustedAngle = ((angle * 2) - 78) * -1; // Yes, a magic number
	//float adjustedAngle = (angle * 2) * -1;

	cameraServo.SetAngle(adjustedAngle);
}

float Shooter::SpeedLookup(float angle)
{
	float rval = 0;
	int i;
	bool foundIt = false;

//	printf("SpeedLookup angle = %0.2f\n", angle);
	
	for(i = 0; i < SPEED_TABLE_LENGTH-1; i++)
	{
		if(angle < speedTable[i][0])
		{
			foundIt = true;
			rval = 0;
			break;
		}
		else if((angle >= speedTable[i][0]) && (angle < speedTable[i+1][0]))
		{
			// Found it
			// TODO interpolate with the next value
			foundIt = true;
			rval = speedTable[i][1];
			break;
		}
	}

	if (!foundIt)
	{
		// Must be last value in table
		rval = speedTable[SPEED_TABLE_LENGTH-1][1];
	}
	
//	printf("SpeedLookup = %0.2f\n", rval);
	
	return(rval);
}


